import numpy as np
import matplotlib.pyplot as plt

def perspectiveTransform(points, matrix):
    """对点集应用透视变换"""
    points_homogeneous = np.hstack([points, np.ones((points.shape[0], 1))])  # 转换为齐次坐标
    transformed_points = points_homogeneous @ matrix.T  # 进行矩阵变换
    transformed_points /= transformed_points[:, 2].reshape(-1, 1)  # 归一化
    return transformed_points[:, :2]  # 返回转换后的二维坐标


def perspectiveTransform2(point, matrix):
    """对点应用透视变换"""
    denominator = matrix[2][0] * point[0] + matrix[2][1] * point[1] + matrix[2][2]

    output = [0, 0]

    output[0] = (matrix[0][0] * point[0] + matrix[0][1] * point[1] + matrix[0][2]) / denominator
    output[1] = (matrix[1][0] * point[0] + matrix[1][1] * point[1] + matrix[1][2]) / denominator

    return output

# 手动设定示例点集
middle_point = [320, 240]

size1 = 50
size2 = 120

points1 = np.array([
    middle_point,
    [middle_point[0] - size1, middle_point[1] - size1], [middle_point[0], middle_point[1] - size1], [middle_point[0] + size1, middle_point[1] - size1],
    [middle_point[0] - size1, middle_point[1]], [middle_point[0] + size1, middle_point[1]],
    [middle_point[0] - size1, middle_point[1] + size1], [middle_point[0], middle_point[1] + size1], [middle_point[0] + size1, middle_point[1] + size1]
])

points2 = np.array([
    [middle_point[0] - size2, middle_point[1] - size2], [middle_point[0], middle_point[1] - size2], [middle_point[0] + size2, middle_point[1] - size2],
    [middle_point[0] - size2, middle_point[1]], [middle_point[0] + size2, middle_point[1]],
    [middle_point[0] - size2, middle_point[1] + size2], [middle_point[0], middle_point[1] + size2], [middle_point[0] + size2, middle_point[1] + size2]
])

points3 = np.array([
    [320, 326],
    [138, 417],
    [502, 417],
    [215, 237],
    [425, 237]

    # [0, 0],
    # [640, 0],
    # [0, 480],
    # [640, 480]
])

# 设定一个示例透视变换矩阵
# perspective_matrix = np.array([
#     [74.55332183837890625000, 219.60966491699218750000, -23202.91796875000000000000],
#     [0.00000000000000000000, 291.53118896484375000000, -35381.82421875000000000000],
#     [-0.00000000000000000033, 0.31187123060226440430, 1.00000000000000000000]
# ])

# perspective_matrix = np.array([
#     [74.55332183837890625000, 219.60966491699218750000, -23202.91796875000000000000],
#     [0, 291.53118896484375000000, -35381.82421875000000000000],
#     [0, 0.5, 1]
# ])

# perspective_matrix = np.array([
#     [0.34843206405639648438, 0.00000000000000000263, -0.00000000000000177636],
#     [-0.00000000000000000976, 0.55555558204650878906, 13.41463375091552734375],
#     [-0.00000000000000000017, 0.00298102991655468941, 1.00000000000000000000]
# ])

perspective_matrix = np.loadtxt("./mat1.txt", dtype=float)

# 计算透视变换后的点集

transformed_points1 = [perspectiveTransform2(point, perspective_matrix) for point in points1]
transformed_points1 = np.array(transformed_points1)

transformed_points2 = [perspectiveTransform2(point, perspective_matrix) for point in points2]
transformed_points2 = np.array(transformed_points2)

transformed_points3 = [perspectiveTransform2(point, perspective_matrix) for point in points3]
transformed_points3 = np.array(transformed_points3)

# 绘制原始点集和透视变换后的点集
fig, axes = plt.subplots(1, 2, figsize=(12, 5))
axes[0].scatter(points1[:, 0], points1[:, 1], color="blue")
axes[0].scatter(points2[:, 0], points2[:, 1], color="red")
axes[0].scatter(points3[:, 0], points3[:, 1], color="green")
axes[0].set_title("Original Points")
# axes[0].legend()
axes[0].grid(True)
axes[0].invert_yaxis()  # 反转 y 轴
axes[0].set_aspect("equal")
axes[0].set_box_aspect(1)

axes[1].scatter(transformed_points1[:, 0], transformed_points1[:, 1], color="blue")
axes[1].scatter(transformed_points2[:, 0], transformed_points2[:, 1], color="red")
axes[1].scatter(transformed_points3[:, 0], transformed_points3[:, 1], color="green")
axes[1].set_title("Perspective Transformed Points")
# axes[1].legend()
axes[1].grid(True)
axes[1].invert_yaxis()  # 反转 y 轴
axes[1].set_aspect("equal")
axes[1].set_box_aspect(1)

plt.show()